#ifndef CRRTTREE_H
#define CRRTTREE_H

#include <QObject>
#include <vector>

#include "CRRTNode.h"
#include "CKDTree.h"
#include "CPoint.h"


class CRRTTree : public QObject
{
    Q_OBJECT
public:
    explicit CRRTTree(QObject *parent = nullptr);
    
    CRRTTree(double goal_bias, double step_size, double max_step_size);

    void grow(const CRRTNode &node);

    void setSizeX(unsigned int size)    {   m_size_x = size;    }
    void setSizeY(unsigned int size)    {   m_size_y = size;    }

    void setGoalBias(double bias)       {   m_goal_bias = bias; }
    double getGoalBias()    const       {   return m_goal_bias; }

    void setStepSize(double size)       {   m_step_size = size; }
    double getStepSize()    const       {   return m_step_size; }

    void setMaxStepSize(double size)    {   m_max_step_size = size; }
    double getMaxStepSize() const       {   return m_max_step_size; }

    size_t size() const                 {   return m_nodes.size();      }

    void clear()                        {   m_nodes.clear();        }

    CRRTNode * getNodeFrontPtr()        {   return (m_nodes.empty() ? nullptr : &m_nodes.front());  }
    CRRTNode * getLastNodePtr()         {   return &m_nodes.back(); }

    std::vector<CPoint2D> getPath();

    CRRTNode generateTreeNode(bool &state);

signals:

private:

    CPoint2D generateRandomPoint();

    CPoint2D generateOffsetPoint(const CPoint2D &start_point, const CPoint2D &end_point);

private:

    unsigned int m_size_x;
    unsigned int m_size_y;

    double m_goal_bias;
    double m_step_size;
    double m_max_step_size;

    CPoint2D  m_start_point;
    CPoint2D  m_goal_point;

    std::deque<CRRTNode> m_nodes;
};

#endif // CRRTTREE_H
